﻿#include <sys/types.h>
#include <sys/socket.h>
#include <sys/ioctl.h>
#include <linux/wireless.h>
#include <string.h> 
#include <stdio.h>
#include <stdlib.h>
#include <netinet/in.h>
#include <unistd.h>
#include <arpa/inet.h>
#include "mav_inc/common/mavlink.h"
#include<time.h>
// 串口头函数
#include <pthread.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <termios.h>

#define NET_PORT 8765    // 调试8765 真机PORT 1243
#define NET_IP "192.168.43.116" // 真机IP 192.168.137.101
#define BUFFER_LENGTH 1024
//#define NET_IP "39.104.91.189" //谷歌DNS

// 串口预定义
#define BAUDRATE        B115200
#define UART_DEVICE     "/dev/ttyS1"   // S1--> COM2，类推。
#define FALSE  -1
#define TRUE   0

int sockfd, uartfd;
struct sockaddr_in servaddr;
mavlink_message_t msg;   // 得到sysid,componentid为全局参数。

/**
*@brief  设置串口通信速率
*@param  uartfd     类型 int  打开串口的文件句柄
*@param  speed  类型 int  串口速度
*@return  void
*/
const int speed_arr[] = { B115200, B38400, B19200, B9600, B4800, B2400, B1200, B300,
				   B115200, B38400, B19200, B9600, B4800, B2400, B1200, B300, };
const int name_arr[] = { 115200, 38400, 19200, 9600, 4800, 2400, 1200,  300,
				  115200, 38400, 19200, 9600, 4800, 2400, 1200,  300, };

void set_speed(int fd, int speed)
{
	int   i;
	int   status;
	struct termios   Opt;
	tcgetattr(fd, &Opt); //读取终端参数
	for (i = 0; i < sizeof(speed_arr) / sizeof(int); i++) {
		if (speed == name_arr[i]) {
			tcflush(fd, TCIOFLUSH);
			cfsetispeed(&Opt, speed_arr[i]); //设置输入波特率 
			cfsetospeed(&Opt, speed_arr[i]); //设置输出波特率  
			status = tcsetattr(fd, TCSANOW, &Opt);  //设置终端参数
			if (status != 0) {
				perror("tcsetattr fd1");
				return;
			}
			tcflush(fd, TCIOFLUSH);
		}
	}
}

/**
*@brief   设置串口数据位，停止位和效验位
*@param  uartfd     类型  int  打开的串口文件句柄
*@param  databits 类型  int 数据位   取值 为 7 或者8
*@param  stopbits 类型  int 停止位   取值为 1 或者2
*@param  parity  类型  int  效验类型 取值为N,E,O,,S
*/
int set_parity(int fd, int databits, int stopbits, int parity)
{
	struct termios options;
	if (tcgetattr(fd, &options) != 0) {
		perror("SetupSerial 1");
		return(FALSE);
	}


	options.c_cflag &= ~CSIZE;
	switch (databits) /*设置数据位数*/
	{
	case 7:
		options.c_cflag |= CS7;
		break;
	case 8:
		options.c_cflag |= CS8;
		break;
	default:
		fprintf(stderr, "Unsupported data size\n"); return (FALSE);
	}
	switch (parity)
	{
	case 'n':
	case 'N':
		options.c_cflag &= ~PARENB;   /* Clear parity enable */
		options.c_iflag &= ~INPCK;     /* Enable parity checking */
		break;
	case 'o':
	case 'O':
		options.c_cflag |= (PARODD | PARENB); /* 设置为奇效验*/
		options.c_iflag |= INPCK;             /* Disnable parity checking */
		break;
	case 'e':
	case 'E':
		options.c_cflag |= PARENB;     /* Enable parity */
		options.c_cflag &= ~PARODD;   /* 转换为偶效验*/
		options.c_iflag |= INPCK;       /* Disnable parity checking */
		break;
	case 'S':
	case 's':  /*as no parity*/
		options.c_cflag &= ~PARENB;
		options.c_cflag &= ~CSTOPB; break;
	default:
		fprintf(stderr, "Unsupported parity\n");
		return (FALSE);
	}
	/* 设置停止位*/
	switch (stopbits)
	{
	case 1:
		options.c_cflag &= ~CSTOPB;
		break;
	case 2:
		options.c_cflag |= CSTOPB;
		break;
	default:
		fprintf(stderr, "Unsupported stop bits\n");
		return (FALSE);
	}
	/* Set input parity option */
	if (parity != 'n')
		options.c_iflag |= INPCK;
	tcflush(fd, TCIFLUSH);

	//时间控制，原始模式有效
	options.c_cc[VTIME] = 10 * 10; /* 设置超时10 seconds(单位1/10s 100ms)*/
	options.c_cc[VMIN] = 0;

	//     options.c_cc[VMIN] = FRAME_MAXSIZE;   //阻塞条件下有效
		//如果不是开发终端之类的，只是串口传输数据，而不需要串口来处理，那么使用原始模式(Raw Mode)方式来通讯，~ICANON设置串口为原始模式,设置方式如下：
	options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);  /*Input*/
	options.c_oflag &= ~OPOST;   /*Output*/

	/* Update the options and do it NOW */
	if (tcsetattr(fd, TCSANOW, &options) != 0)
	{
		perror("SetupSerial 3");
		return (FALSE);
	}

	return (TRUE);
}

void uart_open()
{
//	printf("正在打开串口...\n");
	uartfd = open(UART_DEVICE, O_RDWR | O_NOCTTY/*|O_NDELAY*/);

	if (uartfd < 0) {
		perror(UART_DEVICE);
		exit(1);
	}

	printf("正在打开串口...\n");
	set_speed(uartfd, 115200);
	if (set_parity(uartfd, 8, 1, 'N') == FALSE) {
		printf("Set Parity Error\n");
		exit(0);
	}
}

// 向串口发送命令
void uart_write(char* buf)
{
	//写数据
	write(uartfd, buf, strlen(buf));
	printf("串口发送长度：%d\n\n", strlen(buf));
}

// 解析串口数据，
int uart_parse(char* buf)
{
	int res = 0;
	return res;
}

// 一键起飞函数
void one_key_take_off()
{
	///* Send Status */
	uint8_t *send_buf;
	mavlink_message_t msg_armed, msg_take_off, msg_stable_mode;
	// MAV_COMP_ID_USER1 25
	mavlink_msg_command_long_pack(1, 25, &msg_armed, msg.sysid, 0, 400, 0, 
		0, 0, 0, 0, 0, 0, 1);
	uint16_t len = mavlink_msg_to_send_buffer(send_buf, &msg_armed);
	size_t bytes_sent = sendto(sockfd, send_buf, len, 0, (struct sockaddr*)&servaddr, sizeof(struct sockaddr_in));
	sleep(1);
	mavlink_msg_command_long_pack(1, 25, &msg_stable_mode, msg.sysid, 0, 176, 0,
		0, 0, 0, 0, 0, 0, 1);
	len = mavlink_msg_to_send_buffer(send_buf, &msg_stable_mode);
	bytes_sent = sendto(sockfd, send_buf, len, 0, (struct sockaddr*)&servaddr, sizeof(struct sockaddr_in));
	sleep(1);
	mavlink_msg_command_long_pack(1, 25, &msg_take_off, msg.sysid, 0, 22, 0, 
		0, 0, 0, 0, 0, 0, 1.2);
	len = mavlink_msg_to_send_buffer(send_buf, &msg_take_off);
	bytes_sent = sendto(sockfd, send_buf, len, 0, (struct sockaddr*)&servaddr, sizeof(struct sockaddr_in));
	
}

// int main(int argc, char *argv[])
void* uart_thread(void* arg)
{
	printf("串口等待读数...\n");
	int  c = 0, res;
	char  buf[1024];
	while (1) {
		//读数据
		res = read(uartfd, buf, 255);

		if (res == 0)
			continue;

		printf("串口读数长度：%d\n", res);

		buf[res] = 0;
		printf("%s\n", buf);

		// 收到起飞命令，向无人机发送起飞命令(mavlink):解锁、起飞、自主模式三步。
		if (uart_parse(buf))
		{
			one_key_take_off();
		}
		//退出
		if (buf[0] == '#') break;

	}

	printf("串口关闭...\n");
	close(uartfd);

	return 0;
}

//————————————————
//版权声明：本文为CSDN博主「eric_pyt@qq.com」的原创文章，遵循CC 4.0 BY - SA版权协议，转载请附上原文出处链接及本声明。
//原文链接：https ://blog.csdn.net/pyt1234567890/article/details/107726427

int get_char_value(char ch)
{
	if (ch >= '0' && ch <= '9')
		return ch - '0';
	else if (ch >= 'a' && ch <= 'z')
		return ch - 'a' + 10;
	else if (ch >= 'A' && ch <= 'Z')
		return ch - 'A' + 10;
}

uint8_t char2int(uint8_t hi, uint8_t lo)
{
	return get_char_value(hi)*16 + get_char_value(lo);
}


int scan_connect_read_parse()
{
	struct iwreq wreq;
	//struct iw_statistics stats;
	//char buffer[32];
	int in_len = 0;
	bool is_landing = false, is_landed = false;

	//memset(buffer, 0, 32);
	//memset(&stats, 0, sizeof(stats));
	memset(&wreq, 0, sizeof(wreq));
	strcpy(wreq.ifr_name, "wlx70f11c3abcd4");   // ens33  // wlx70f11c3abcd4   // wlp2s0

	//建立socket，用于接收来自ioctl函数的消息。SOCK_DGRAM是UDP。如果是SOCK_STREAM,则是TCP。
	if ((sockfd = socket(AF_INET, SOCK_STREAM, 0)) == -1)
	{
		perror("无法创建连接！");
		//exit(EXIT_FAILURE);
		return 0;
	}
	

	in_len = sizeof(struct sockaddr_in);
	/*设置默认服务器的信息*/
	servaddr.sin_family = AF_INET;
	servaddr.sin_port = htons(NET_PORT);
	servaddr.sin_addr.s_addr = inet_addr(NET_IP);
	memset(servaddr.sin_zero, 0, sizeof(servaddr.sin_zero));

	/*connect 函数*/
	if (connect(sockfd, (struct sockaddr*)&servaddr, in_len) < 0)
	{
		printf("无法连接无人机\n ");
		close(sockfd);
		return 0;
	}
	else
	{
		printf("连接无人机成功！\n");
	}
	
	uint8_t buf[BUFFER_LENGTH];
	ssize_t recsize;
	socklen_t fromlen = sizeof(servaddr);
	uint8_t temp = 0;
	while (1)
	{
		memset(buf, 0, BUFFER_LENGTH);
		recsize = recvfrom(sockfd, (void*)buf, BUFFER_LENGTH, 0, (struct sockaddr*)&servaddr, &fromlen);
		if (recsize > 0)
		{
			// Something received - print out all bytes and parse packet
			mavlink_status_t status;
			mavlink_global_position_int_t global_position;
			mavlink_servo_output_raw_t servo_output_raw;
			mavlink_extended_sys_state_t extended_sys_state;

			time_t timep;
			time(&timep);
			printf("\n时间：%s", ctime(&timep));

			printf("接收字节数：%d  \n", (int)recsize);
			// 调试时，串口数据保存为文本，每字节写成了两个数，如 254('FE')，
			//实际上可能是255一个字节为一个字节，此时代码略有不同。
			//for (int i = 0; i < recsize; i ++)
			//{
			//	temp = (char)buf[i];

			for (int i = 0; i < recsize; i+=2)
			{
				temp = (char)char2int(buf[i],buf[i+1]);
				printf("%02X ", temp);
				//temp = buf[i];
				//printf("整型：%02X ", temp);
				uint8_t res = mavlink_parse_char(MAVLINK_COMM_0, temp, &msg, &status);
				if (res == 1)
				{
					// Packet received
					printf("Received packet: SEQ: %d, SYS: %d, COMP: %d, LEN: %d, MSG ID: %d\n",
						msg.seq, msg.sysid, msg.compid, msg.len, msg.msgid);

					switch (msg.msgid) 
					{
					case MAVLINK_MSG_ID_GLOBAL_POSITION_INT: // ID for GLOBAL_POSITION_INT
					
					// Get all fields in payload (into global_position)
					mavlink_msg_global_position_int_decode(&msg, &global_position);
					printf("高度：%d mm \n", global_position.relative_alt);
					
					break;
					case MAVLINK_MSG_ID_SERVO_OUTPUT_RAW:
					// Get just one field from payload
					mavlink_msg_servo_output_raw_decode(&msg,&servo_output_raw);
					//printf("转速：%d, %d, %d, %d, %d, %d, %d, %d \n", 
					//	servo_output_raw.servo1_raw, servo_output_raw.servo2_raw, 
					//	servo_output_raw.servo3_raw, servo_output_raw.servo4_raw,
					//	servo_output_raw.servo5_raw, servo_output_raw.servo6_raw,
					//	servo_output_raw.servo6_raw, servo_output_raw.servo8_raw);
					if (servo_output_raw.servo1_raw = 1066 && servo_output_raw.servo2_raw == 1066)
					{
						if (is_landing && is_landed)
						{
							// 飞机着陆且桨已停止，向串口发送锁定指令。
							char* command_land = (char*)buf;
							uart_write(command_land);
							is_landing = false;
						}
					}
					break;
					case MAVLINK_MSG_ID_EXTENDED_SYS_STATE: // ID for EXTENDED_SYS_STATE 245
					// Get all fields in payload (into global_position)
					mavlink_msg_extended_sys_state_decode(&msg, &extended_sys_state);
					switch (extended_sys_state.landed_state)
					{
					case MAV_LANDED_STATE_ON_GROUND:
//							is_landing = false;
						is_landed = true;    // 设置成功着陆标志。
						printf("着陆成功！ \n");
						break;
					case MAV_LANDED_STATE_TAKEOFF:
						is_landed = false;
						is_landing = false;
						printf("正在起飞…… \n");
						break;
					case 	MAV_LANDED_STATE_LANDING:
						is_landing = true;
						printf("正在着陆…… \n");
						break;
					default:
						break;
					}
							
					break;

					default:
						break;
					}
				}
			}
			//sleep(0.2); // Sleep 200 ms
		}
		else
		{
			close(sockfd);
			return -1;
		}
		//		memset(buf, 0, BUFFER_LENGTH);
	}
	/* 学习、测试用的代码
	//	//ioctl获取Signal level
	//	wreq.u.data.pointer = &stats;
	//	wreq.u.data.length = sizeof(iw_statistics);
	////	if (ioctl(sockfd, 0X8946, &wreq) == -1) {
	//	if (ioctl(sockfd, SIOCGIWESSID, &wreq) == -1) {
	//		perror("Error performing SIOCGIWSTATS");
	//		close(sockfd);
	//		exit(EXIT_FAILURE);
	//	}
	//	else {
	//		// IW_QUAL_DBM表示Level + Noise are dBmm
	//		printf("Signal level%s is %d%s.\n", (stats.qual.updated & IW_QUAL_DBM ? " (in dBm)" : ""),
	//			(signed char)stats.qual.level,
	//			(stats.qual.updated & IW_QUAL_LEVEL_UPDATED ? " (updated)" : ""));
	//	}
	//
	//	//ioctl获取essid
	//	wreq.u.essid.pointer = buffer;//如果不写这行可能会错误
	//	wreq.u.essid.length = 32;
	////	if (ioctl(sockfd, 0X8946, &wreq) == -1) {
	//	if (ioctl(sockfd, SIOCGIWESSID, &wreq) == -1) {
	//			perror("IOCTL SIOCGIWESSID Failed,error");
	//		exit(2);
	//	}
	//	else {
	//		//printf("IOCTL SIOCGIWESSID Successfull\n");
	//		printf("The essid is:%s\n", wreq.u.essid.pointer);        //network name
	//	}
	//
	//
	//	in_port_t port;
	//
	//	char *address = inet_ntoa(servaddr.sin_addr);
	//	port = servaddr.sin_port;
	//
	//	printf("inet addr: %s\n", address);
	//	printf("inet port: %d\n", port);
	*/
	close(sockfd);
	return 0;
}

int main(int argc, char* argv[])
{
	int res;
	//pthread_t thread1;
	//uart_open();
	//res = pthread_create(&thread1, NULL, uart_thread, NULL);
	//if (res != 0) {
	//	printf("create uart_thread thread fail\n");
	//	exit(res);
	//}

	while(1)
	{
		scan_connect_read_parse();
	}
	return 0;
}
